#include "action.h"
#include <stdio.h>
#include <math.h>
#define PI 3.1415926535897932384626433832795

void jump(double step,double l,int  data[20])
{
    while(step>1024)
    step-=1024;
    // rise up arms
  //  data[14]=data[16]=((double)+60-(double)60*(step/1024))*scale_p_degree;

    //left leg
    data[1]=-(-l+(double)l*(step/1024))*scale_p_degree;
    data[4]=-data[1];

    data[2]=-(-l+(double)l*(step/1024))*scale_p_degree;

    data[6]=-data[2];

    //right leg

    data[8]=(-l+(double)l*(step/1024))*scale_p_degree;
    //data[8]=30.0*scale_p_degree;
    data[11]=-data[8];

    data[9]=(-l+(double)l*(step/1024))*scale_p_degree;
    data[13]=-data[9];
}

void jump_l(double step,double l,int data[20])//d-step=55,l=7
{
    double theta,fi;
    while(step>1024)
    step-=1024;

    step=1024-step;
    theta=asin(l/24)+1.3*step/1024;
    fi=PI/2-acos(sin(theta)-l/12);

    data[3]=PI/2*scale_p_degree/(PI/180);
    data[10]=-PI/2*scale_p_degree/(PI/180);
    //left leg
    data[1]=theta*scale_p_degree/(PI/180);
    data[4]=-fi*scale_p_degree/(PI/180);

    //right leg

    data[8]=-data[1];
    data[11]=-data[4];


    data[14]=data[16]=0;
}

void jump_kneel(double step,double l,int data[20])
{//能站住
     while(step>1024)
    step-=1024;

    step=1024-step;

    data[1]=-(-l+(double)l*(step/1024))*scale_p_degree;
    data[4]=-data[1];
    //right leg

    data[8]=-data[1];
    data[11]=-data[4];
}

void balence(int data[20])
{
    const double l=20;
   // double l1=(cos(abs(data[1])/scale_p_degree)+cos(abs(data[4])/scale_p_degree))*(sin(abs(data[2])/scale_p_degree));
   // double l2=(cos(abs(data[8])/scale_p_degree)+cos(abs(data[11])/scale_p_degree))*(sin(abs(data[9])/scale_p_degree));

}
